//
//  PMPivotJoint.m
//  PezMonkey
//
//  Created by Spence DiNicolantonio on 10/31/09.
//  Copyright 2009 Necrosoft. All rights reserved.
//

#import "PMPivotJoint.h"
#import "PMBody.h"


@implementation PMPivotJoint


#pragma mark -
#pragma mark Init

/**
 * Returns a PMPivotJoint object initialized by anchoring two given rigid bodys at 
 * a given pivot point. The pivot point should be given in world coordinates.
 */
- (id)initWithBodyA:(PMBody *)aBody bodyB:(PMBody *)otherBody pivot:(PMVect)pivot {
	if (![super init])
		return nil;
	
	// allocate and initialize cpJoint
	joint = cpPivotJointNew(aBody.cpBody, otherBody.cpBody, pivot);
	
	// set body references
	self.bodyA = aBody;
	self.bodyB = otherBody;
	
	return self;
}


#pragma mark -
#pragma mark Getters

/**
 * Returns the receiver's pivot point in world coordinates.
 */
- (PMVect)pivot {
	PMVect anchor1 = ((cpPivotJoint *) joint)->anchr1;
	return (PMVect) cpvadd(cpvrotate(anchor1, self.bodyA.cpBody->rot), self.bodyA.cpBody->p);
}


#pragma mark -
#pragma mark Setters

/**
 * Sets the receiver's pivot to a given point.
 */
- (void)setPivot:(PMVect)pivot {
	((cpPivotJoint *) joint)->anchr1 = cpvunrotate(cpvsub(pivot, self.bodyA.cpBody->p), 
												   self.bodyA.cpBody->rot);
	((cpPivotJoint *) joint)->anchr2 = cpvunrotate(cpvsub(pivot, self.bodyB.cpBody->p), 
												   self.bodyB.cpBody->rot);
}

@end
